#include <stdio.h>
#include <pthread.h>
#include <dirent.h>
#include <errno.h>
#include <unistd.h>
#include "Stack.h"
#include "QuickSort.h"
#include "Node.h"
#include <omp.h>
#include <math.h>
#include <string.h>
#include <Queue.h>
#include <riscv_vector.h>

#define BLOCK_X 16
#define BLOCK_Y 16
#define BLOCK_SIZE 256

typedef struct
{
  int x;
  int y;
} int2;

typedef struct
{
  float x;
  float y;
} float2;

typedef struct
{
  float x;
  float y;
  float z;
} float3;

typedef struct
{
  float x;
  float y;
  float z;
  float w;
} float4;

int fib(int n)
{
  if (n <= 2)
  {
    return 1;
  }
  else
  {
    return fib(n - 1) + fib(n - 2);
  }
}

void tensor_print(float *tensor, int size)
{
  for (int i = 0; i < size; i++)
    printf("%f\n", tensor[i]);
  tensor[0] = 1.5;
}

void GetRanges_tile(int tile_id, int *grid, float *depths, int2 *rect_min, int2 *rect_max, int size, int2 *indices_min, int2 *indices_max, int *res, int *res_size)
{
  memset(res, -1, size);
  res_size[0] = 0;
  int tile_x = tile_id / grid[0];
  int tile_y = tile_id % grid[0];
  int index;         // 高斯的索引
  Node *root = NULL; // 二叉排序树
  if (tile_x + tile_y < 24)
  {
    for (int i = 0; i < size; i++)
    {
      index = indices_min[i].x;
      if (rect_min[index].x > tile_x)
        break;
      if (rect_min[index].x <= tile_x && rect_min[index].y <= tile_y && rect_max[index].x >= tile_x && rect_max[index].y >= tile_y)
      {
        res[res_size[0]++] = index;
        root = insertNodewithValue(root, index, depths[index]);
        // printf("index:%d,depth:%f\n",index, depths[index]);
      }
    }
  }
  else
  {
    for (int i = size - 1; i >= 0; i--)
    {
      index = indices_max[i].x;
      if (rect_max[index].x < tile_x)
        break;
      if (rect_min[index].x <= tile_x && rect_min[index].y <= tile_y && rect_max[index].x >= tile_x && rect_max[index].y >= tile_y)
      {
        res[res_size[0]++] = index;
        root = insertNodewithValue(root, index, depths[index]);
        // printf("index:%d,depth:%f\n",index, depths[index]);
      }
    }
  }
  int num = 0;
  inOrderTraversalwithValue(root, res, &num); // 中序遍历将高斯索引加入返回值
}

// res变为二维的, res_size
void GetRanges_tile2(int tile_id, int *grid, float *depths, int2 *rect_min, int2 *rect_max, int size, int2 *indices_min, int2 *indices_max, int *res, int *res_size)
{
  // 还能换种思路：for(0:size) for(rect_min.x:rect_min.y) for(rect_max.x:rect_max.y) 将当前高斯加入对应tile的二叉排序树
  // memset(res,-1,size);
  // res_size[tile_id] = 0;
  int tiles_num = grid[0] * grid[0];
  // printf("%d\n",tiles_num);
  // #pragma omp parallel for num_threads(40)
  for (tile_id = 0; tile_id < tiles_num; tile_id++)
  {
    // printf("%d\n",tile_id);
    int tile_x = tile_id / grid[0];
    int tile_y = tile_id % grid[0];
    int index;         // 高斯的索引
    Node *root = NULL; // 二叉排序树
    if (tile_x + tile_y < 24)
    {
      for (int i = 0; i < size; i++)
      {
        index = indices_min[i].x;
        if (depths[index] == 0)
          continue;
        // printf("i: %d, index: %d\n",i,index);
        if (rect_min[index].x > tile_x)
          break;
        if (rect_min[index].x <= tile_x && rect_min[index].y <= tile_y && rect_max[index].x >= tile_x && rect_max[index].y >= tile_y)
        {
          res_size[tile_id]++;
          root = insertNodewithValue(root, index, depths[index]);
        }
      }
    }
    else
    {
      for (int i = size - 1; i >= 0; i--)
      {
        index = indices_max[i].x;
        if (depths[index] == 0)
          continue;
        if (rect_max[index].x < tile_x)
          break;
        if (rect_min[index].x <= tile_x && rect_min[index].y <= tile_y && rect_max[index].x >= tile_x && rect_max[index].y >= tile_y)
        {
          res_size[tile_id]++;
          root = insertNodewithValue(root, index, depths[index]);
        }
      }
    }
    int num = 0;
    inOrderTraversalwithValue(root, res + tile_id * 50000, &num); // 中序遍历将高斯索引加入返回值
  }
}

// 只将对应tile的ranges传入，所以此处不需要range_index
void render_tile(int tile_id, float *bg, int *grid, float2 *means2D, float4 *conic_opacity, float3 *rgb, int *ranges, int ranges_size, float *image, int H, int W)
{
  // printf("%d %ld\n", tile_id, grid);
  int tile_x = tile_id / grid[0];
  int tile_y = tile_id % grid[0];
  int2 pix_min = {tile_x * BLOCK_X, tile_y * BLOCK_Y};
#pragma omp parallel for num_threads(40)
  for (int x = pix_min.x; x < BLOCK_X + pix_min.x; x++)
  {
    for (int y = pix_min.y; y < BLOCK_Y + pix_min.y; y++)
    {
      // printf("%d,%d\n",x,y);
      float T = 1;
      float3 RGB = {0.0f, 0.0f, 0.0f};
      for (int i = 0; i < ranges_size; i++)
      {
        int index = ranges[i];
        float2 d = {means2D[index].x - x, means2D[index].y - y};
        float power = -0.5f * (conic_opacity[index].x * d.x * d.x + conic_opacity[index].z * d.y * d.y) - (conic_opacity[index].y * d.x * d.y);
        float alpha = fmin(0.99f, conic_opacity[index].w * exp(power));
        if (alpha < 1.0f / 255.0f)
          continue;
        RGB.x += alpha * T * rgb[index].x;
        RGB.y += alpha * T * rgb[index].y;
        RGB.z += alpha * T * rgb[index].z;
        T *= (1 - alpha);
        if (T < 0.0001f)
          break;
      }
      memcpy(image + (y * W + x) * 3, &RGB, 3 * sizeof(float));
    }
  }
}

/*-------------------------预处理-------------------------*/

const float SH_C0 = 0.28209479177387814f;
const float SH_C1 = 0.4886025119029199f;
const float SH_C2[] = {
    1.0925484305920792f,
    -1.0925484305920792f,
    0.31539156525252005f,
    -1.0925484305920792f,
    0.5462742152960396f};
const float SH_C3[] = {
    -0.5900435899266435f,
    2.890611442640554f,
    -0.4570457994644658f,
    0.3731763325901154f,
    -0.4570457994644658f,
    1.445305721320277f,
    -0.5900435899266435f};

float3 transformPoint4x3(const float3 p, const float *matrix)
{
  float3 transformed = {
      matrix[0] * p.x + matrix[4] * p.y + matrix[8] * p.z + matrix[12],
      matrix[1] * p.x + matrix[5] * p.y + matrix[9] * p.z + matrix[13],
      matrix[2] * p.x + matrix[6] * p.y + matrix[10] * p.z + matrix[14],
  };
  return transformed;
}

float4 transformPoint4x4(const float3 p, const float *matrix)
{
  float4 transformed = {
      matrix[0] * p.x + matrix[4] * p.y + matrix[8] * p.z + matrix[12],
      matrix[1] * p.x + matrix[5] * p.y + matrix[9] * p.z + matrix[13],
      matrix[2] * p.x + matrix[6] * p.y + matrix[10] * p.z + matrix[14],
      matrix[3] * p.x + matrix[7] * p.y + matrix[11] * p.z + matrix[15]};
  return transformed;
}

int max(int a, int b)
{
  if (a > b)
    return a;
  return b;
}

int min(int a, int b)
{
  if (a < b)
    return a;
  return b;
}

bool in_frustum(int idx,
                const float *orig_points,
                const float *viewmatrix,
                const float *projmatrix,
                bool prefiltered,
                float3 *p_view)
{
  float3 p_orig = {orig_points[3 * idx], orig_points[3 * idx + 1], orig_points[3 * idx + 2]};
  // Bring points to screen space
  // float4 p_hom = transformPoint4x4(p_orig, projmatrix);
  // float p_w = 1.0f / (p_hom.w + 0.0000001f);
  // float3 p_proj = { p_hom.x * p_w, p_hom.y * p_w, p_hom.z * p_w };
  p_view[0] = transformPoint4x3(p_orig, viewmatrix);
  if (p_view[0].z <= 0.2f) // || ((p_proj.x < -1.3 || p_proj.x > 1.3 || p_proj.y < -1.3 || p_proj.y > 1.3)))
  {
    return false;
  }
  return true;
}

void transposeMatrix3x3(float mat[3][3])
{
  float temp;
  for (int i = 0; i < 3; i++)
  {
    for (int j = i + 1; j < 3; j++)
    {
      temp = mat[i][j];
      mat[i][j] = mat[j][i];
      mat[j][i] = temp;
    }
  }
}

void MatMul3x3(float mat1[3][3], float mat2[3][3], float result[3][3])
{
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      result[i][j] = 0;
      for (int k = 0; k < 3; k++)
      {
        result[i][j] += mat1[i][k] * mat2[k][j];
      }
    }
  }
}

float3 computeCov2D(const float3 mean, float focal_x, float focal_y, float tan_fovx, float tan_fovy, const float *cov3D, const float *viewmatrix)
{

  float3 t = transformPoint4x3(mean, viewmatrix);
  // printf("t: %f,%f,%f\n",t.x,t.y,t.z);
  const float limx = 1.3f * tan_fovx;
  const float limy = 1.3f * tan_fovy;
  const float txtz = t.x / t.z;
  const float tytz = t.y / t.z;
  t.x = fmin(limx, fmax(-limx, txtz)) * t.z;
  t.y = fmin(limy, fmax(-limy, tytz)) * t.z;
  // printf("t: %f,%f,%f\n",t.x,t.y,t.z);
  float J[3][3] = {
      focal_x / t.z, 0.0f, -(focal_x * t.x) / (t.z * t.z),
      0.0f, focal_y / t.z, -(focal_y * t.y) / (t.z * t.z),
      0, 0, 0};

  // printf("J: ");
  // for(int i = 0;i < 3;i++){
  //   for(int j = 0;j < 3;j++){
  //     printf("%f ",J[i][j]);
  //   }
  // }
  // printf("\n");

  float W[3][3] = {
      viewmatrix[0], viewmatrix[4], viewmatrix[8],
      viewmatrix[1], viewmatrix[5], viewmatrix[9],
      viewmatrix[2], viewmatrix[6], viewmatrix[10]};

  // printf("W: ");
  // for(int i = 0;i < 3;i++){
  //   for(int j = 0;j < 3;j++){
  //     printf("%f ",W[i][j]);
  //   }
  // }
  // printf("\n");

  float T[3][3] = {0};
  MatMul3x3(J, W, T); // 跟glm中的T=W*J是反过来的

  // printf("T: ");
  // for(int i = 0;i < 3;i++){
  //   for(int j = 0;j < 3;j++){
  //     printf("%f ",T[i][j]);
  //   }
  // }
  // printf("\n");

  float Vrk[3][3] = {
      cov3D[0], cov3D[1], cov3D[2],
      cov3D[1], cov3D[3], cov3D[4],
      cov3D[2], cov3D[4], cov3D[5]};

  float TT[3][3];
  memcpy(TT, T, 9 * sizeof(float));

  float VrkT[3][3];
  memcpy(VrkT, Vrk, 9 * sizeof(float));

  transposeMatrix3x3(TT);
  transposeMatrix3x3(VrkT);
  float temp[3][3] = {0};
  float cov[3][3] = {0};
  MatMul3x3(VrkT, TT, temp);
  MatMul3x3(T, temp, cov);

  // Apply low-pass filter: every Gaussian should be at least
  // one pixel wide/high. Discard 3rd row and column.
  cov[0][0] += 0.3f;
  cov[1][1] += 0.3f;
  float3 result = {cov[0][0], cov[0][1], cov[1][1]};
  return result;
}

void computeCov3D(const float3 scale, float mod, const float4 rot, float *cov3D)
{
  // Create scaling matrix
  float S[3][3] = {0};
  S[0][0] = mod * scale.x;
  S[1][1] = mod * scale.y;
  S[2][2] = mod * scale.z;

  // Normalize quaternion to get valid rotation
  float4 q = rot; // / glm::length(rot);
  float r = q.x;
  float x = q.y;
  float y = q.z;
  float z = q.w;

  // Compute rotation matrix from quaternion
  float R[3][3] = {1.f - 2.f * (y * y + z * z), 2.f * (x * y + r * z), 2.f * (x * z - r * y),
                   2.f * (x * y - r * z), 1.f - 2.f * (x * x + z * z), 2.f * (y * z + r * x),
                   2.f * (x * z + r * y), 2.f * (y * z - r * x), 1.f - 2.f * (x * x + y * y)};

  float M[3][3] = {0};
  MatMul3x3(S, R, M);
  // Compute 3D world covariance matrix Sigma
  float MT[3][3];
  memcpy(MT, M, 9 * sizeof(float));
  transposeMatrix3x3(MT);
  float Sigma[3][3];
  MatMul3x3(MT, M, Sigma);
  // Covariance is symmetric, only store upper right
  cov3D[0] = Sigma[0][0];
  cov3D[1] = Sigma[0][1];
  cov3D[2] = Sigma[0][2];
  cov3D[3] = Sigma[1][1];
  cov3D[4] = Sigma[1][2];
  cov3D[5] = Sigma[2][2];
}

void getRect(const float2 p, float max_radius, int2 *rect_min, int2 *rect_max, int *grid)
{
  rect_min[0].x = min(grid[0], max((int)0, (int)((p.x - max_radius) / BLOCK_X)));
  rect_min[0].y = min(grid[0], max((int)0, (int)((p.y - max_radius) / BLOCK_Y)));

  rect_max[0].x = min(grid[0], max((int)0, (int)((p.x + max_radius + BLOCK_X - 1) / BLOCK_X)));
  rect_max[0].y = min(grid[0], max((int)0, (int)((p.y + max_radius + BLOCK_Y - 1) / BLOCK_Y)));
}

float length(float3 vec)
{
  return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
}

float3 computeColorFromSH(int idx, int deg, int max_coeffs, const float3 *means, float3 campos, const float *shs)
{
  // The implementation is loosely based on code for
  // "Differentiable Point-Based Radiance Fields for
  // Efficient View Synthesis" by Zhang et al. (2022)
  float3 pos = means[idx];
  float3 dir = {pos.x - campos.x, pos.y - campos.y, pos.z - campos.z};
  // printf("original dir:%f,%f,%f\n", dir.x,dir.y,dir.z);
  // printf("pos:%f,%f,%f\n", pos.x,pos.y,pos.z);
  // printf("campos:%f,%f,%f\n", campos.x,campos.y,campos.z);
  float dir_length = length(dir);
  dir.x = dir.x / dir_length;
  dir.y = dir.y / dir_length;
  dir.z = dir.z / dir_length;
  // printf("dir:%f,%f,%f\n", dir.x,dir.y,dir.z);
  // printf("dir_length:%f\n", dir_length);
  float3 *sh = ((float3 *)shs) + idx * max_coeffs;
  float3 result = {SH_C0 * sh[0].x, SH_C0 * sh[0].y, SH_C0 * sh[0].z};

  // for(int i = 0;i < 16;i++){
  //   printf("%f, %f, %f\n", sh[i].x,sh[i].y,sh[i].z);
  // }
  // printf("result:%f,%f,%f\n", result.x,result.y,result.z);
  if (deg > 0)
  {
    float x = dir.x;
    float y = dir.y;
    float z = dir.z;
    result.x = result.x - SH_C1 * y * sh[1].x + SH_C1 * z * sh[2].x - SH_C1 * x * sh[3].x;
    result.y = result.y - SH_C1 * y * sh[1].y + SH_C1 * z * sh[2].y - SH_C1 * x * sh[3].y;
    result.z = result.z - SH_C1 * y * sh[1].z + SH_C1 * z * sh[2].z - SH_C1 * x * sh[3].z;
    // printf("deg0:%f,%f,%f\n", result.x,result.y,result.z);
    if (deg > 1)
    {
      float xx = x * x, yy = y * y, zz = z * z;
      float xy = x * y, yz = y * z, xz = x * z;
      result.x = result.x +
                 SH_C2[0] * xy * sh[4].x +
                 SH_C2[1] * yz * sh[5].x +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].x +
                 SH_C2[3] * xz * sh[7].x +
                 SH_C2[4] * (xx - yy) * sh[8].x;

      result.y = result.y +
                 SH_C2[0] * xy * sh[4].y +
                 SH_C2[1] * yz * sh[5].y +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].y +
                 SH_C2[3] * xz * sh[7].y +
                 SH_C2[4] * (xx - yy) * sh[8].y;

      result.z = result.z +
                 SH_C2[0] * xy * sh[4].z +
                 SH_C2[1] * yz * sh[5].z +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].z +
                 SH_C2[3] * xz * sh[7].z +
                 SH_C2[4] * (xx - yy) * sh[8].z;
      // printf("deg1:%f,%f,%f\n", result.x,result.y,result.z);
      if (deg > 2)
      {
        result.x = result.x +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].x +
                   SH_C3[1] * xy * z * sh[10].x +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].x +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].x +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].x +
                   SH_C3[5] * z * (xx - yy) * sh[14].x +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].x;

        result.y = result.y +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].y +
                   SH_C3[1] * xy * z * sh[10].y +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].y +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].y +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].y +
                   SH_C3[5] * z * (xx - yy) * sh[14].y +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].y;

        result.z = result.z +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].z +
                   SH_C3[1] * xy * z * sh[10].z +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].z +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].z +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].z +
                   SH_C3[5] * z * (xx - yy) * sh[14].z +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].z;
        // printf("deg2:%f,%f,%f\n", result.x,result.y,result.z);
      }
    }
  }
  result.x += 0.5f;
  result.y += 0.5f;
  result.z += 0.5f;
  // RGB colors are clamped to positive values. If values are
  // clamped, we need to keep track of this for the backward pass.
  // clamped[3 * idx + 0] = (result.x < 0);
  // clamped[3 * idx + 1] = (result.y < 0);
  // clamped[3 * idx + 2] = (result.z < 0);
  float3 res = {fmax(result.x, 0.0f), fmax(result.y, 0.0f), fmax(result.z, 0.0f)};
  return res;
}

float3 computeColorFromSH_compressed(int idx, int deg, int max_coeffs, const float3 *means, float3 campos, const float *shs, const long long *sh_indices)
{
  // The implementation is loosely based on code for
  // "Differentiable Point-Based Radiance Fields for
  // Efficient View Synthesis" by Zhang et al. (2022)
  float3 pos = means[idx];
  float3 dir = {pos.x - campos.x, pos.y - campos.y, pos.z - campos.z};
  // printf("original dir:%f,%f,%f\n", dir.x,dir.y,dir.z);
  // printf("pos:%f,%f,%f\n", pos.x,pos.y,pos.z);
  // printf("campos:%f,%f,%f\n", campos.x,campos.y,campos.z);
  float dir_length = length(dir);
  dir.x = dir.x / dir_length;
  dir.y = dir.y / dir_length;
  dir.z = dir.z / dir_length;

  float3 *sh = ((float3 *)shs) + sh_indices[idx] * max_coeffs;
  float3 result = {SH_C0 * sh[0].x, SH_C0 * sh[0].y, SH_C0 * sh[0].z};
  // if(idx == 15736){
  //   printf("sh:%f,%f,%f\n", sh[0].x,sh[0].y,sh[0].z);
  //   printf("dir:%f,%f,%f\n", dir.x,dir.y,dir.z);
  //   printf("dir_length:%f\n", dir_length);
  // }
  // for(int i = 0;i < 16;i++){
  //   printf("%f, %f, %f\n", sh[i].x,sh[i].y,sh[i].z);
  // }
  // printf("result:%f,%f,%f\n", result.x,result.y,result.z);
  if (deg > 0)
  {
    float x = dir.x;
    float y = dir.y;
    float z = dir.z;
    result.x = result.x - SH_C1 * y * sh[1].x + SH_C1 * z * sh[2].x - SH_C1 * x * sh[3].x;
    result.y = result.y - SH_C1 * y * sh[1].y + SH_C1 * z * sh[2].y - SH_C1 * x * sh[3].y;
    result.z = result.z - SH_C1 * y * sh[1].z + SH_C1 * z * sh[2].z - SH_C1 * x * sh[3].z;
    // if(idx == 15736)
    //   printf("result:%f,%f,%f\n", result.x,result.y,result.z);

    if (deg > 1)
    {
      float xx = x * x, yy = y * y, zz = z * z;
      float xy = x * y, yz = y * z, xz = x * z;
      result.x = result.x +
                 SH_C2[0] * xy * sh[4].x +
                 SH_C2[1] * yz * sh[5].x +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].x +
                 SH_C2[3] * xz * sh[7].x +
                 SH_C2[4] * (xx - yy) * sh[8].x;

      result.y = result.y +
                 SH_C2[0] * xy * sh[4].y +
                 SH_C2[1] * yz * sh[5].y +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].y +
                 SH_C2[3] * xz * sh[7].y +
                 SH_C2[4] * (xx - yy) * sh[8].y;

      result.z = result.z +
                 SH_C2[0] * xy * sh[4].z +
                 SH_C2[1] * yz * sh[5].z +
                 SH_C2[2] * (2.0f * zz - xx - yy) * sh[6].z +
                 SH_C2[3] * xz * sh[7].z +
                 SH_C2[4] * (xx - yy) * sh[8].z;
      // if(idx == 15736)
      //   printf("result:%f,%f,%f\n", result.x,result.y,result.z);
      if (deg > 2)
      {
        result.x = result.x +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].x +
                   SH_C3[1] * xy * z * sh[10].x +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].x +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].x +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].x +
                   SH_C3[5] * z * (xx - yy) * sh[14].x +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].x;

        result.y = result.y +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].y +
                   SH_C3[1] * xy * z * sh[10].y +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].y +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].y +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].y +
                   SH_C3[5] * z * (xx - yy) * sh[14].y +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].y;

        result.z = result.z +
                   SH_C3[0] * y * (3.0f * xx - yy) * sh[9].z +
                   SH_C3[1] * xy * z * sh[10].z +
                   SH_C3[2] * y * (4.0f * zz - xx - yy) * sh[11].z +
                   SH_C3[3] * z * (2.0f * zz - 3.0f * xx - 3.0f * yy) * sh[12].z +
                   SH_C3[4] * x * (4.0f * zz - xx - yy) * sh[13].z +
                   SH_C3[5] * z * (xx - yy) * sh[14].z +
                   SH_C3[6] * x * (xx - 3.0f * yy) * sh[15].z;
        // if(idx == 15736)
        //   printf("result:%f,%f,%f\n", result.x,result.y,result.z);
      }
    }
  }
  result.x += 0.5f;
  result.y += 0.5f;
  result.z += 0.5f;
  // RGB colors are clamped to positive values. If values are
  // clamped, we need to keep track of this for the backward pass.
  // clamped[3 * idx + 0] = (result.x < 0);
  // clamped[3 * idx + 1] = (result.y < 0);
  // clamped[3 * idx + 2] = (result.z < 0);

  float3 res = {fmax(result.x, 0.0f), fmax(result.y, 0.0f), fmax(result.z, 0.0f)};
  return res;
}

float ndc2Pix(float v, int S)
{
  return ((v + 1.0f) * S - 1.0f) * 0.5f;
}

void preprocess(
    int idx,
    int P, int D, int M,
    const float *orig_points,
    const float3 *scales,
    const float scale_modifier,
    const float4 *rotations,
    const float *opacities,
    const float *shs,
    bool *clamped,
    const float *cov3D_precomp,
    const float *colors_precomp,
    const float *viewmatrix,
    const float *projmatrix,
    const float3 *cam_pos,
    const int W, int H,
    const float tan_fovx, float tan_fovy,
    const float focal_x, float focal_y,
    int *radii,
    float2 *points_xy_image,
    float *depths,
    float *cov3Ds,
    float *rgb,
    float4 *conic_opacity,
    int *grid,
    int *tiles_touched,
    bool prefiltered,
    int2 *p_rect_min,
    int2 *p_rect_max)
{
#pragma omp parallel for num_threads(40)
  for (idx = 0; idx < P; idx++)
  {
    // printf("campos:%f,%f,%f\n", cam_pos->x,cam_pos->y,cam_pos->z);
    radii[idx] = 0;
    tiles_touched[idx] = 0;

    float3 p_view;
    if (!in_frustum(idx, orig_points, viewmatrix, projmatrix, prefiltered, &p_view))
      continue;

    float3 p_orig = {orig_points[3 * idx], orig_points[3 * idx + 1], orig_points[3 * idx + 2]};
    float4 p_hom = transformPoint4x4(p_orig, projmatrix);
    // printf("p_orig:%f,%f,%f\n", p_orig.x,p_orig.y,p_orig.z);
    // printf("p_view:%f,%f,%f\n", p_view.x,p_view.y,p_view.z);
    // printf("p_hom:%f,%f,%f\n", p_hom.x,p_hom.y,p_hom.z);
    float p_w = 1.0f / (p_hom.w + 0.0000001f);
    float3 p_proj = {p_hom.x * p_w, p_hom.y * p_w, p_hom.z * p_w};
    const float *cov3D;
    computeCov3D(scales[idx], scale_modifier, rotations[idx], cov3Ds + idx * 6);
    cov3D = cov3Ds + idx * 6;
    // printf("cov3D: ");
    // for(int n = 0;n < 6;n++)
    //   printf("%f ",cov3D[n]);
    // printf("\n");
    float3 cov = computeCov2D(p_orig, focal_x, focal_y, tan_fovx, tan_fovy, cov3D, viewmatrix);
    // printf("cov2D:%f,%f,%f\n",cov.x,cov.y,cov.z);
    float det = (cov.x * cov.z - cov.y * cov.y);
    if (det == 0.0f)
      continue;
    float det_inv = 1.f / det;
    float3 conic = {cov.z * det_inv, -cov.y * det_inv, cov.x * det_inv};
    // printf("conic:%f,%f,%f\n",conic.x,conic.y,conic.z);
    float mid = 0.5f * (cov.x + cov.z);
    float lambda1 = mid + sqrt(fmax(0.1f, mid * mid - det));
    float lambda2 = mid - sqrt(fmax(0.1f, mid * mid - det));
    float my_radius = ceil(3.f * sqrt(fmax(lambda1, lambda2)));
    float2 point_image = {ndc2Pix(p_proj.x, W), ndc2Pix(p_proj.y, H)};
    // printf("point_image:%f,%f\n",point_image.x,point_image.y);
    int2 rect_min, rect_max;
    getRect(point_image, my_radius, &rect_min, &rect_max, grid);
    if ((rect_max.x - rect_min.x) * (rect_max.y - rect_min.y) == 0)
      continue;

    float3 result = computeColorFromSH(idx, D, M, (float3 *)orig_points, *cam_pos, shs);
    rgb[idx * 3 + 0] = result.x;
    rgb[idx * 3 + 1] = result.y;
    rgb[idx * 3 + 2] = result.z;

    depths[idx] = p_view.z;
    radii[idx] = my_radius;
    points_xy_image[idx] = point_image;

    conic_opacity[idx].x = conic.x;
    conic_opacity[idx].y = conic.y;
    conic_opacity[idx].z = conic.z;
    conic_opacity[idx].w = opacities[idx];

    tiles_touched[idx] = (rect_max.y - rect_min.y) * (rect_max.x - rect_min.x);
    p_rect_min[idx] = rect_min;
    p_rect_max[idx] = rect_max;

    // break;
  }
}

/*----------------压缩后的模型----------------*/

void preprocess_compressed(
    int P, int D, int M,
    const float *orig_points,
    const float3 *scales,
    const float *scale_factors,
    const float scale_modifier,
    const float4 *rotations,
    const float *opacities,
    const float *shs,
    const float *viewmatrix,
    const float *projmatrix,
    const float3 *cam_pos,
    const int W, int H,
    const float tan_fovx, float tan_fovy,
    const float focal_x, float focal_y,
    int *radii,
    float2 *points_xy_image,
    float *depths,
    float *cov3Ds,
    float *rgb,
    float4 *conic_opacity,
    int *grid,
    int *tiles_touched,
    bool prefiltered,
    int2 *p_rect_min,
    int2 *p_rect_max,
    long long *sh_indices,
    long long *g_indices,
    float *image)
{
#pragma omp parallel for num_threads(50)
  for (int idx = 0; idx < P; idx++)
  {
    // printf("campos:%f,%f,%f\n", cam_pos->x,cam_pos->y,cam_pos->z);
    radii[idx] = 0;
    tiles_touched[idx] = 0;
    // printf("viewmatrix: ");
    // for(int i = 0;i < 16;i++)
    //   printf("%f ", viewmatrix[i]);
    // printf("\n");
    // printf("projmatrix: ");
    // for(int i = 0;i < 16;i++)
    //   printf("%f ", projmatrix[i]);
    // printf("\n");
    float3 p_view;
    if (!in_frustum(idx, orig_points, viewmatrix, projmatrix, prefiltered, &p_view))
      continue;
    float3 p_orig = {orig_points[3 * idx], orig_points[3 * idx + 1], orig_points[3 * idx + 2]};
    float4 p_hom = transformPoint4x4(p_orig, projmatrix);
    // if(idx == 356){
    //   printf("p_orig:%f,%f,%f\n", p_orig.x,p_orig.y,p_orig.z);
    //   printf("p_view:%f,%f,%f\n", p_view.x,p_view.y,p_view.z);
    //   printf("p_hom:%f,%f,%f\n", p_hom.x,p_hom.y,p_hom.z);
    // }
    float p_w = 1.0f / (p_hom.w + 0.0000001f);
    float3 p_proj = {p_hom.x * p_w, p_hom.y * p_w, p_hom.z * p_w};
    const float *cov3D;
    // printf("g_indices[idx]:%ld\n",g_indices[idx]);
    computeCov3D(scales[g_indices[idx]], scale_factors[idx] * scale_modifier, rotations[g_indices[idx]], cov3Ds + idx * 6);
    cov3D = cov3Ds + idx * 6;
    // printf("cov3D: ");
    // for(int n = 0;n < 6;n++)
    //   printf("%f ",cov3D[n]);
    // printf("\n");
    float3 cov = computeCov2D(p_orig, focal_x, focal_y, tan_fovx, tan_fovy, cov3D, viewmatrix);
    // if(idx == 356)
    //   printf("cov2D:%f,%f,%f\n",cov.x,cov.y,cov.z);
    float det = (cov.x * cov.z - cov.y * cov.y);
    if (det == 0.0f)
      continue;
    float det_inv = 1.0f / det;
    float3 conic = {cov.z * det_inv, -cov.y * det_inv, cov.x * det_inv};
    // printf("conic:%f,%f,%f\n",conic.x,conic.y,conic.z);
    float mid = 0.5f * (cov.x + cov.z);
    float lambda1 = mid + sqrt(fmax(0.1f, mid * mid - det));
    float lambda2 = mid - sqrt(fmax(0.1f, mid * mid - det));
    float my_radius = ceil(3.f * sqrt(fmax(lambda1, lambda2)));

    float2 point_image = {ndc2Pix(p_proj.x, W), ndc2Pix(p_proj.y, H)};
    // printf("point_image:%f,%f\n",point_image.x,point_image.y);
    int2 rect_min, rect_max;
    getRect(point_image, my_radius, &rect_min, &rect_max, grid);
    if ((rect_max.x - rect_min.x) * (rect_max.y - rect_min.y) == 0)
      continue;

    float3 result = computeColorFromSH_compressed(idx, D, M, (float3 *)orig_points, *cam_pos, shs, sh_indices);
    rgb[idx * 3 + 0] = result.x;
    rgb[idx * 3 + 1] = result.y;
    rgb[idx * 3 + 2] = result.z;

    depths[idx] = p_view.z;
    radii[idx] = my_radius;
    points_xy_image[idx] = point_image;

    conic_opacity[idx].x = conic.x;
    conic_opacity[idx].y = conic.y;
    conic_opacity[idx].z = conic.z;
    conic_opacity[idx].w = opacities[idx];

    tiles_touched[idx] = (rect_max.y - rect_min.y) * (rect_max.x - rect_min.x);
    p_rect_min[idx] = rect_min;
    p_rect_max[idx] = rect_max;
    // break;
  }
}

void duplicateWithKeys(int P, int2 *rect_min, int2 *rect_max, float *depths, int *offsets, long long *gaussian_keys, int *gaussian_values, int *grid, int *radii)
{
#pragma omp parallel for num_threads(40)
  for (int idx = 0; idx < P; idx++)
  {
    if (radii[idx] > 0)
    {
      // printf("\r idx: %d ", idx);
      int offset = (idx == 0) ? 0 : offsets[idx - 1];
      for (int y = rect_min[idx].y; y < rect_max[idx].y; y++)
      {
        for (int x = rect_min[idx].x; x < rect_max[idx].x; x++)
        {
          long long key = y * grid[0] + x;
          key <<= 32;
          key |= *((unsigned int *)&depths[idx]);
          gaussian_keys[offset] = key;
          gaussian_values[offset] = idx;
          offset++;
        }
      }
    }
  }
}

void GetTileRanges(int L, long long *gaussian_keys, int2 *ranges)
{
#pragma omp parallel for num_threads(40)
  for (int idx = 0; idx < L; idx++)
  {
    // printf("\r idx: %d ", idx);
    long long key = gaussian_keys[idx];
    int currentile = key >> 32;
    if (idx == 0)
      ranges[currentile].x = 0;
    else
    {
      long long prevtile = gaussian_keys[idx - 1] >> 32;
      if (currentile != prevtile)
      {
        ranges[prevtile].y = idx;
        ranges[currentile].x = idx;
      }
    }
    if (idx == L - 1)
      ranges[currentile].y = L;
    // printf("idx: %d, currentile: %d\n", idx, currentile);
    // break;
  }
}

void render(int *grid, int *gaussian_idx_list, float2 *means2D, float4 *conic_opacity, float3 *rgb, int2 *ranges, float *image, int H, int W)
{
  int tile_total = grid[0] * grid[1];
#pragma omp parallel for num_threads(40)
  for (int tile_id = 0; tile_id < tile_total; tile_id++)
  {
    // int tile_id = 930;
    // printf("\r %d", tile_id);
    int tile_x = tile_id % grid[0];
    int tile_y = tile_id / grid[0];
    int2 pix_min = {tile_x * BLOCK_X, tile_y * BLOCK_Y};
    // #pragma omp parallel for num_threads(40)
    for (int x = pix_min.x; x < BLOCK_X + pix_min.x; x++)
    {
      for (int y = pix_min.y; y < BLOCK_Y + pix_min.y; y++)
      {
        // printf("%d, %d\n",x,y);
        float T = 1.0f;
        float RGB[3] = {0};
        for (int i = ranges[tile_id].x; i < ranges[tile_id].y; i++)
        {
          int index = gaussian_idx_list[i];
          float2 d = {means2D[index].x - x, means2D[index].y - y};
          float power = -0.5f * (conic_opacity[index].x * d.x * d.x + conic_opacity[index].z * d.y * d.y) - (conic_opacity[index].y * d.x * d.y);
          if (power > 0.0f)
            continue;
          float alpha = fmin(0.99f, conic_opacity[index].w * expf(power));
          if (alpha < 1.0f / 255.0f)
            continue;
          RGB[0] += alpha * T * rgb[index].x;
          RGB[1] += alpha * T * rgb[index].y;
          RGB[2] += alpha * T * rgb[index].z;
          T *= (1 - alpha);
          // if(y * W + x == 233746)
          //   printf("T: %f, gaussian_id: %d, alpha: %f, power: %f, xy: %f, %f, con_o: %f, %f, %f, %f, rgb: %f, %f, %f\n",
          //   T, index, alpha, power, means2D[index].x, means2D[index].y, conic_opacity[index].x, conic_opacity[index].y,
          //   conic_opacity[index].z, conic_opacity[index].w, rgb[index].x, rgb[index].y, rgb[index].z);
          if (T < 0.0001f)
            break;
        }
        RGB[0] = fmin(1.0f, RGB[0]);
        RGB[1] = fmin(1.0f, RGB[1]);
        RGB[2] = fmin(1.0f, RGB[2]);
        memcpy(image + (y * W + x) * 3, &RGB, 3 * sizeof(float));
      }
    }
    // break;
  }
}

/* -(x + scalar) */
vfloat32m4_t vector_add_scalar_and_negative(float *x, float scalar, float *res, int length)
{
  vfloat32m4_t vx, dx;
  while (length > 0)
  {
    size_t vl = vsetvl_e32m4(length);
    vx = vle32_v_f32m4(x, vl);
    dx = vfadd_vf_f32m4(vx, scalar, vl);
    dx = vfneg_v_f32m4(dx, vl);
    vse32_v_f32m4(res, dx, vl);
    length -= vl;
    x += vl;
    res += vl;
  }
  return dx;
}

float xy_base[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
void renderV2(int *grid, int *gaussian_idx_list, float2 *means2D, float4 *conic_opacity, float3 *rgb, int2 *ranges, float *image, int H, int W)
{
  memset(image, 0, W * H * 3 * sizeof(float));
  int tile_total = grid[0] * grid[1];
  const int batch_size_gaussian = 256;
  #pragma omp parallel for num_threads(40)
  for (int tile_id = 0; tile_id < tile_total; tile_id++)
  {
    int tile_x = tile_id % grid[0];
    int tile_y = tile_id / grid[0];
    int2 pix_min = {tile_x * BLOCK_X, tile_y * BLOCK_Y};
    int progress = 0;
    int collect_gaussian_id[batch_size_gaussian];
    float accum_T[256] = {0};
    bool done[256] = {0};
    int toDo = ranges[tile_id].y - ranges[tile_id].x;
    int rounds = ((ranges[tile_id].y - ranges[tile_id].x + BLOCK_SIZE - 1) / BLOCK_SIZE);
    int num_done = 0;
    for (int i = 0; i < BLOCK_SIZE; i++)
      accum_T[i] = 1.0f;
    /* 计算坐标差值 */
    float dx[BLOCK_X] = {0};
    float dy[BLOCK_Y] = {0};
    // for(int n = 0;n < BLOCK_X;n++){
    //   vx[n] = xy_base[n] + pix_min.x;
    //   vy[n] = xy_base[n] + pix_min.y;
    // }
    for (int i = 0; i < rounds; i++, toDo -= BLOCK_SIZE)
    {
      if (num_done == BLOCK_SIZE)
        break;
      progress = i * BLOCK_SIZE;
      /* 取出一个batch的高斯用于渲染 */
      // for(int j = progress; j < progress + batch_size_gaussian; j++){
      //   if(ranges[tile_id].x + j < ranges[tile_id].y)
      //     collect_gaussian_id[j - progress] = gaussian_idx_list[ranges[tile_id].x + j];
      //   else
      //     break;
      // }
      memcpy(collect_gaussian_id,
             gaussian_idx_list + ranges[tile_id].x + progress,
             min(batch_size_gaussian, toDo) * sizeof(float));

      /* 遍历高斯 */
      for (int k = 0; k < min(BLOCK_SIZE, toDo); k++)
      {
        int index = collect_gaussian_id[k];
        /* 计算所有的坐标差值 */
        // for(int n = 0;n < BLOCK_X;n++){
        //   dx[n] = means2D[index].x - vx[n];
        //   dy[n] = means2D[index].y - vy[n];
        // }

        vfloat32m4_t vdx = vector_add_scalar_and_negative(xy_base, pix_min.x - means2D[index].x, dx, BLOCK_X);
        vfloat32m4_t vdy = vector_add_scalar_and_negative(xy_base, pix_min.y - means2D[index].y, dy, BLOCK_Y);
        /*-----test-----*/

        float power[BLOCK_SIZE];
        vfloat32m4_t power_row = vfmv_v_f_f32m4(0, 16); // power的一行
        vfloat32m4_t vdx_square = vfmul_vv_f32m4(vdx, vdx, 16);
        // vfloat32m4_t vdy_square = vfmul_vv_f32m4(vdy, vdy, 16);
        // 一次计算一行
        for (int row = 0; row < 16; row++)
        {
          // power_row = -0.5 * (con_o.x * vdx_square + con_o.z * vdy_square) - (con_o.y * vdx * dy[row])
          vfloat32m4_t power_row_1 = vfadd_vf_f32m4(vfmul_vf_f32m4(vdx_square, -0.5f * conic_opacity[index].x, 16),
                                                    -0.5f * conic_opacity[index].z * dy[row] * dy[row],
                                                    16);
          power_row = vfsub_vv_f32m4(power_row_1, vfmul_vf_f32m4(vdx, dy[row] * conic_opacity[index].y, 16), 16);
          vse32_v_f32m4(power + row * 16, power_row, 16);
        }
        /*-----test-----*/
        for (int pix_id = 0; pix_id < BLOCK_SIZE; pix_id++)
        {
          if (done[pix_id])
            continue;
          int index_x = pix_id % BLOCK_X;
          int x = index_x + pix_min.x;
          int index_y = pix_id / BLOCK_Y;
          int y = index_y + pix_min.y;
          if (done[pix_id])
            continue;
          // float power = -0.5f * (conic_opacity[index].x * dx[index_x] * dx[index_x] + conic_opacity[index].z * dy[index_y] * dy[index_y]) - (conic_opacity[index].y * dx[index_x] * dy[index_y]);
          if (power[pix_id] > -0.01f || power[pix_id] < -5.55f)
            continue;

          /* conic_opacity[index].w为opacity,介于[0,1],只会缩小指数结果 */
          float alpha = conic_opacity[index].w * expf(power[pix_id]);
          if (alpha < 1.0f / 255.0f)
            continue;
          int pix_location = (y * W + x) * 3;
          image[pix_location] += alpha * accum_T[pix_id] * rgb[index].x;
          image[pix_location + 1] += alpha * accum_T[pix_id] * rgb[index].y;
          image[pix_location + 2] += alpha * accum_T[pix_id] * rgb[index].z;

          image[pix_location] = fmin(1.0f, image[pix_location]);
          image[pix_location + 1] = fmin(1.0f, image[pix_location + 1]);
          image[pix_location + 2] = fmin(1.0f, image[pix_location + 2]);

          if (image[pix_location] == 1.0f && image[pix_location + 1] == 1.0f && image[pix_location + 2] == 1.0f)
            done[pix_id] = true;

          accum_T[pix_id] *= (1 - alpha);
          if (accum_T[pix_id] < 0.0001f)
          {
            done[pix_id] = true;
            ++num_done;
            continue;
          }
        }
      }
    }
    // break;
  }
}

// void renderV2(int *grid,int* gaussian_idx_list, float2 *means2D, float4 *conic_opacity, float3* rgb, int2* ranges, float *image, int H, int W){
//   memset(image, 0, W * H * 3 * sizeof(float));
//   int tile_total = grid[0] * grid[1];
//   const int batch_size_gaussian = 256;
//   #pragma omp parallel for num_threads(40)
//   for(int tile_id = 0; tile_id < tile_total; tile_id++){
//     int tile_x = tile_id % grid[0];
//     int tile_y = tile_id / grid[0];
//     int2 pix_min = {tile_x * BLOCK_X, tile_y * BLOCK_Y};
//     int progress = 0;
//     int collect_gaussian_id[batch_size_gaussian];
//     float accum_T[256] = {0};
//     bool done[256] = {0};
//     int toDo = ranges[tile_id].y - ranges[tile_id].x;
//     int rounds = ((ranges[tile_id].y - ranges[tile_id].x + BLOCK_SIZE - 1) / BLOCK_SIZE);
//     int num_done = 0;
//     for(int i = 0;i < BLOCK_SIZE;i++)
//       accum_T[i] = 1.0f;
//     for(int i = 0;i < rounds;i++, toDo-=BLOCK_SIZE){
//       if(num_done == BLOCK_SIZE)
//         break;
//       progress = i * BLOCK_SIZE;
//       /* 取出一个batch的高斯用于渲染 */
//       for(int j = progress; j < progress + batch_size_gaussian; j++){
//         if(ranges[tile_id].x + j < ranges[tile_id].y)
//           collect_gaussian_id[j - progress] = gaussian_idx_list[ranges[tile_id].x + j];
//         else
//           break;
//       }
//       float2 d;
//       /* 遍历高斯 */
//       for(int k = 0;k < min(BLOCK_SIZE, toDo);k++){
//         int index = collect_gaussian_id[k];
//         // printf("index: %d\n",index);
//         /* 计算所有的坐标差值 */
//         for(int pix_id = 0;pix_id < BLOCK_SIZE;pix_id++){
//           if(done[pix_id])
//             continue;
//           int x = pix_id % BLOCK_X + pix_min.x;
//           int y = pix_id / BLOCK_Y + pix_min.y;
//           d.x = means2D[index].x - x;
//           d.y = means2D[index].y - y;
//         /* 每个像素都对高斯 k 做计算 */
//           if(done[pix_id])
//             continue;
//           float power = -0.5f * (conic_opacity[index].x * d.x * d.x + conic_opacity[index].z * d.y * d.y) - (conic_opacity[index].y * d.x * d.y);
//           if (power > 0.0f)
// 				    continue;
//           float alpha = fmin(0.99f, conic_opacity[index].w * expf(power));
//           if (alpha < 1.0f / 255.0f)
//             continue;
//           int pix_location = (y * W + x) * 3;
//           image[pix_location]     += alpha * accum_T[pix_id] * rgb[index].x;
//           image[pix_location + 1] += alpha * accum_T[pix_id] * rgb[index].y;
//           image[pix_location + 2] += alpha * accum_T[pix_id] * rgb[index].z;
//           accum_T[pix_id] *= (1 - alpha);
//           if(accum_T[pix_id] < 0.0001f){
//             done[pix_id] = true;
//             ++num_done;
//             continue;
//           }

//         }
//       }
//     }
//     // break;
//   }
// }

// 交换数组元素及其索引
void swap(long long *array, long long *indices, int i, int j)
{
  long long temp1 = array[i];
  array[i] = array[j];
  array[j] = temp1;
  long long temp2 = indices[i];
  indices[i] = indices[j];
  indices[j] = temp2;
}

// 分割函数，返回分割点的索引
int partition(long long *array, long long *indices, int low, int high)
{
  long long pivot = array[high];
  int i = low - 1;
  for (int j = low; j <= high - 1; j++)
  {
    if (array[j] < pivot)
    {
      i++;
      swap(array, indices, i, j);
    }
  }
  swap(array, indices, i + 1, high);
  return i + 1;
}

// 快速排序函数
void quickSort(long long *array, long long *indices, int length)
{
  int stack[length];
  int top = -1;
  stack[++top] = 0;
  stack[++top] = length - 1;

  while (top >= 0)
  {
    int high = stack[top--];
    int low = stack[top--];
    int pi = partition(array, indices, low, high);
    if (pi - 1 > low)
    {
      stack[++top] = low;
      stack[++top] = pi - 1;
    }

    if (pi + 1 < high)
    {
      stack[++top] = pi + 1;
      stack[++top] = high;
    }
  }
}

// 排序函数接口
void sort(long long *array, int length, long long *indices)
{
  // 调用快速排序
  quickSort(array, indices, length);
}

void c_sort(long long *gaussian_keys, int n, long long *indices, int *tile_gaussian_num, int tile_num, int2 *ranges)
{
  /* 统计每个tile有多少高斯 */
  for (int i = 0; i < n; ++i)
  {
    ++tile_gaussian_num[gaussian_keys[i] >> 32];
  }
  /* 获取索引偏置 */
  int tile_gaussian_num_offset[2500] = {0};
  for (int i = 1; i < 2500; ++i)
  {
    tile_gaussian_num_offset[i] = tile_gaussian_num[i] + tile_gaussian_num_offset[i - 1];
    // printf("tile_gaussian_num_offset: %d\n", tile_gaussian_num_offset[i]);
  }
  long long *collect_gaussian_keys = (long long *)malloc(n * sizeof(long long));
  int tile_offset[2500];
  memcpy(tile_offset, tile_gaussian_num_offset, 2500 * sizeof(int));
  for (int i = 0; i < n; ++i)
  {
    collect_gaussian_keys[--tile_gaussian_num_offset[gaussian_keys[i] >> 32]] = gaussian_keys[i];
    indices[tile_gaussian_num_offset[gaussian_keys[i] >> 32]] = i;
  }
  #pragma omp parallel for num_threads(40)
  for (int tile_id = 0; tile_id < tile_num; ++tile_id)
  {
    if (tile_gaussian_num[tile_id] != 0)
    {
      int offset = tile_id == 0 ? 0 : tile_offset[tile_id - 1];
      int length = tile_offset[tile_id] - offset;
      long long *p_gaussian = collect_gaussian_keys + offset;
      long long *p_indices = indices + offset;
      ranges[tile_id].x = offset;
      ranges[tile_id].y = offset + length;
      // printf("tile_id: %d, offset: %d, length: %d\n",tile_id, offset,length);
      /* tile内排序 */
      sort(p_gaussian, tile_gaussian_num[tile_id], p_indices);
    }
  }
  gaussian_keys = collect_gaussian_keys;
}

void prefixsum(int *data, int *prefix, int length)
{
  prefix[0] = data[0];
  for (int i = 1; i < length; ++i)
    prefix[i] = prefix[i - 1] + data[i];
}
